#include "WPILib.h"
#include "DriverStationLCD.h"
#include "BruceDrive.h"

#define DEBUG(a, ...)   DriverStationLCD::GetInstance()->Printf(a, 1, __VA_ARGS__);DriverStationLCD::GetInstance()->UpdateLCD();

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class Bruce : public SimpleRobot
{
   BruceDrive *bruce; // robot drive system
   DigitalInput *armUpperLimit; // arm upper limit switch
   DigitalInput *armLowerLimit; // arm lower limit switch
   Joystick *rightStick; // joystick 1 (arcade stick or right tank stick)
   Joystick *leftStick; // joystick 2 (tank left stick)
   Joystick *armStick; // joystick 3 to control arm
   DriverStation *ds; // driver station object
   Jaguar *Jag1;
   Jaguar *Jag2;

   enum // Driver Station jumpers to control program operation

   {  ARCADE_MODE = 1, // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
      ENABLE_AUTONOMOUS = 2, // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
   } jumpers;

public:
   /**
    * 
    * 
    * Constructor for this robot subclass.
    * Create an instance of a RobotDrive with left and right motors plugged into PWM
    * ports 1 and 2 on the first digital module.
    */
   Bruce(void)
   {
      ds = DriverStation::GetInstance();
      bruce = new BruceDrive(1, 3, 2, 4); // create robot drive base
      rightStick = new Joystick(1); // create the joysticks
      leftStick = new Joystick(2);
      /*armStick = new Joystick(3);
       armUpperLimit = new DigitalInput(1);	// create the limit switch inputs
       armLowerLimit = new DigitalInput(2);
       */
      Jag1 = new Jaguar(1, 1); // Creates the first Jag
      Jag2 = new Jaguar(2,2);
      //Update the motors at least every 100ms.
      GetWatchdog().SetExpiration(100);
   }

   /**
    * Drive left & right motors for 2 seconds, enabled by a jumper (jumper
    * must be in for autonomous to operate).
    */
   void Autonomous(void)
   {
      GetWatchdog().SetEnabled(false);
      if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
      {
         bruce->Drive(0.5, 0.0); // drive forwards half speed
         Wait(2000); //    for 2 seconds
         bruce->Drive(0.0, 0.0); // stop robot
      }
      GetWatchdog().SetEnabled(true);
   }

   /**
    * Runs the motors under driver control with either tank or arcade steering selected
    * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
    */
   void OperatorControl(void)

   {
      /*ToDo
       * Left analog stick on game controller to control camera rotation
       * Button 2 on controller to fire kicker
       * Button 1 on controller to fire grapling hook
       * Joysticks to control robot direction. Custom class? Or can we just use the RobotDrive?
       * */
      Victor armMotor(5); // create arm motor instance
      while (IsOperatorControl())
      {
         //At the current date this is to have a driving robot. May need to use custom class;
         GetWatchdog().Feed();
         //Set bruce to tank drive
         bruce->TankDrive(leftStick, rightStick);//I think this is all we need. May be mistaken.
         DEBUG(DriverStationLCD::kUser_Line1, "In tank drive")
         ;
         //Check right trigger pressed?
         if (rightStick->GetTrigger())
         {
            //If it is pressed halve right stick value
            DEBUG(DriverStationLCD::kUser_Line1, "Trigger pressed")
            
         } else
         {
            DEBUG(DriverStationLCD::kUser_Line1, "                  ")
         }
         
         //This is our driving code once we have an idea of what we want. Information is within the comments
         //Read right JS value
         /*				float rightStickY;
          rightStickY = rightStick->GetY();
          DEBUG(DriverStationLCD::kUser_Line2, "Right: %0.2f", rightStickY);
          //Read left JS value
          float leftStickY;
          leftStickY = leftStick->GetY();
          DEBUG(DriverStationLCD::kUser_Line3, "Left: %0.2f", leftStickY);
          //Check right trigger pressed?
          if (rightStick->GetTrigger())
          {
          //If it is pressed halve right stick value
          DEBUG("Halve right stick value");
          Jag1->Set(rightStickY * 0.5);
          
          }
          //Check left trigger pressed?
          if (leftStick->GetTrigger())
          {
          //If it is pressed halve left stick value
          DEBUG("Left Stick pressed");
          Jag2->Set(leftStickY * 0.5);
          
          }
          // 5. Read right Jag value //Jag 1 is right Jag
          Jag1->Get();
          // 6. Read left Jag value
          Jag2->Get();
          // 7. New right speed:
          Jag1->Set(rightStickY);
          // 8. New left speed
          Jag2->Set(leftStickY);
          // 9. Calculate new right speed
          Jag1->Get();
          // Use alpha-beta filter if necessary
          // 10. Calculate new left speed
          Jag2->Get();
          // Use alpha-beta filter if necessary
          // 11. Set new right speed on right Jag
          //Jag1->Set();
          // 12. Set new left speed on left Jag
          //Jag2->Set();
          
          */

         // Control the movement of the arm using the joystick
         //			MoveArm();
      }
   }

   /*	
    MoveArm()
    {
    // Use the "Y" value of the arm joystick to control the movement of the arm
    // This is for arm extension control - don't need now
    // if at a limit and telling the arm to move past the limit, don't drive the motor
    if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
    armStickDirection = 0;
    } else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
    armStickDirection = 0;
    }

    // Set the motor value 
    armMotor.Set(armStickDirection);
    
    }
    */
};

START_ROBOT_CLASS(Bruce)
;

